/*
 * servo.c
 *
 *  Created on: 2021��11��28��
 *      Author: iorn
 */
/*
 * servo.c
 *  19�쾵ͷ������
 *  Created on: 2023��12��2��
 *      Author: iorn
 */

#include "zf_common_headfile.h"

void Servo_Init(void)
{
    uint32 i = 0;
    pwm_init(Servo_Pin, Servo_Fre, Servo_Mid);//ATOM 0ģ���ͨ��7 ʹ��P02_7�������PWM  PWMƵ��50HZ  ռ�ձȰٷ�֮1000/GTM_ATOM0_PWM_DUTY_MAX*100
    pwm_set_duty(Servo_Pin, Servo_Mid);
    //    gtm_pwm_init(Servo_Pin, Servo_Fre, 200000);
//    gpio_init(P00_8, GPO, GPIO_HIGH, PUSHPULL);                                   // ��ʼ������Ϊ������� Ĭ�ϸߵ�ƽ
}

void Servo_Out(int Pwm)
{
    Pwm=Pwm>(Servo_Right)?(Servo_Right):Pwm;// ����Ƕ�����
    Pwm=Pwm<(Servo_Left)?(Servo_Left):Pwm;// ����Ƕ�����
    pwm_set_duty(Servo_Pin, Servo_Mid - Pwm);
}

//舵机测试函数
void Servo_Max_Angle(void)
{
    static int Key_data = 0;
    static int Pwm_Max = 0;
//    Key_data=Key_Read();
//    if(Key_data)
//    {
//        system_delay_ms(100);
//        Key_data=Key_Read();
//        switch(Key_data)
//        {
//            case 2:
//                Pwm_Max = 0;
//                break;
//            case 3:
//                Pwm_Max += 50;
//                break;
//            case 4:
//                Pwm_Max -= 50;
//                break;
//            default:
//                break;
//        }
//    }
    Pwm_Max = 700;
    while(1)
    {
        Key_data=Key_Read();
        if(Key_data==1)
        {
            Pwm_Max+=1;
        }
        if(Key_data==2)
        {
            Pwm_Max-=1;
        }
        if(Pwm_Max>999)
            Pwm_Max=999;
        if(Pwm_Max<0)
            Pwm_Max=0;
        oled_show_string(7,2,"    Servo Pwm");
        oled_show_int(80,4,(int32)Pwm_Max,5);
        pwm_set_duty(Servo_Pin, Pwm_Max);

//        while(Pwm_Max>600)
//        {
//            Pwm_Max-=1;
//            oled_show_string(7,2,"    Servo Pwm");
//            oled_show_int(80,4,(int32)Pwm_Max,5);
//            pwm_set_duty(Servo_Pin, Pwm_Max);
//            system_delay_ms(500);
//        }
    }

}

void Servo_Max_Angle2(void)
{
    static int Pwm_Max = 0;
    Pwm_Max = Left_EncoderCount;
    oled_show_string(7,2,"    Servo Pwm");
    oled_show_int(80,4,(int32)Pwm_Max,5);
    Servo_Out(Pwm_Max);
}
